function [q, Q] = adaptprocnoiseupd_sagehusa(updMean, zTilde, cycCnt, b, xPost, xPost1, K, PPost, PPost1, Phi) %#codegen
%ADAPTPROCNOISEUPD_SAGEHUSA Sage-Husa自适应系统噪声估计
%
% Input Arguments:
% # updMean: 更新标志，true表示更新噪声均值和方差，false表示只更新方差（用于噪声均值为零）
% # zTilde: n*1列向量，零均值量测残差的估计
% # cycCnt: 标量，更新周期计数，从1开始
% # b: 标量，遗忘因子，（0<b<1）通常取0.95~0.99，若b=NaN,则采用1/cycCnt计算遗忘因子
% # xPost: m*1列向量，当前周期滤波器更新后的状态向量
% # xPost1: m*1列向量，前一周期滤波器更新后的状态向量
% # K: m*n矩阵，卡尔曼滤波器增益矩阵
% # PPost: m*m矩阵，卡尔曼滤波器更新计算的当前周期状态误差协方差矩阵
% # PPost1: m*m矩阵，卡尔曼滤波器更新计算的前一周期状态误差协方差矩阵
% # Phi: m*m矩阵，离散化之后的状态转移矩阵
%
% Output Arguments:
% # q: m*1列向量，当前周期系统噪声序列均值
% # Q: m*m矩阵，当前周期系统噪声序列协方差矩阵
%
% References:
% # 《应用导航算法工程基础》“系统噪声估计”

persistent p_adaptprocnoiseupd_sagehusa_q p_adaptprocnoiseupd_sagehusa_Q

if isempty(p_adaptprocnoiseupd_sagehusa_q)
    p_adaptprocnoiseupd_sagehusa_q = coder.nullcopy(zeros(size(xPost)));
end
if isempty(p_adaptprocnoiseupd_sagehusa_Q)
    p_adaptprocnoiseupd_sagehusa_Q = coder.nullcopy(zeros(size(xPost, 1)));
end
% 计算比例因子
if isnan(b)
    dn = 1/cycCnt;
else
    dn = (1-b)/(1-b^cycCnt);
end
% 更新系统噪声均值
if updMean
    p_adaptprocnoiseupd_sagehusa_q = (1-dn)*p_adaptprocnoiseupd_sagehusa_q + dn*(xPost - Phi*xPost1);
else
    p_adaptprocnoiseupd_sagehusa_q = zeros(size(xPost));
end
% 更新系统噪声方差
QTmp = dn*((K*zTilde)*zTilde'*K' + PPost - Phi*PPost1*Phi');
QTmp = diag(abs(diag(QTmp)));
p_adaptprocnoiseupd_sagehusa_Q = (1-dn)*p_adaptprocnoiseupd_sagehusa_Q + QTmp;
% 输出
q = p_adaptprocnoiseupd_sagehusa_q;
Q = p_adaptprocnoiseupd_sagehusa_Q;
end